# -*- coding: utf-8 -*-
"""
Created on Fri Apr  1 16:46:10 2022

@author: lizhuang
"""

# -*- coding: utf-8 -*-
"""
Created on Mon Mar 14 20:28:18 2022

@author: lizhuang
"""
import numpy as np
from scipy import integrate
import sviewgui.sview as sv
import pandas as pd
from numpy import sin,cos
import matplotlib.pyplot as plt
plt.rcParams['font.sans-serif']=['SimHei'] #用来正常显示中文标签
plt.rcParams['axes.unicode_minus']=False #用来正常显示负号
class 曲柄连杆():
    def __init__(self,连杆长度=1.2,曲柄长度=0.4,连杆质心=0.6,曲柄质心=0.2,曲柄质量=1.2,连杆质量=3.6,滑块质量=6, \
                 曲柄转动惯量=0.016,连杆转动惯量=0.45,滑块外力x=1000,滑块外力y=0):
        self.时间精度=1000#dt
        self.曲柄角速度=10*np.pi/180
        self.模拟时间长度=20*np.pi/self.曲柄角速度
        self.连杆长度=连杆长度
        self.曲柄长度=曲柄长度
        self.连杆质心=连杆质心
        self.曲柄质心=曲柄质心#质量心到旋转副距离
        self.曲柄质量=曲柄质量
        self.连杆质量=连杆质量
        self.滑块质量=滑块质量
        self.曲柄转动惯量=曲柄转动惯量
        self.连杆转动惯量=连杆转动惯量
        self.曲柄受的扭矩=0
        self.曲柄外力x=0
        self.曲柄外力y=0
        self.曲柄起点加速度x=0
        self.曲柄起点加速度y=0
        self.连杆外力x=0
        self.连杆外力y=0
        self.连杆受扭矩=0
        self.滑块外力x=滑块外力x
        self.滑块外力y=滑块外力y
        self.滑块和固定点延长线夹角=np.pi*3/2
        return
    def r(self,x):
        #Function to compute the acceleration of crank
        #Copyright 2002 Ou Xiuquan
        #Input parameters
        #
        #x1)=rj
        #x[2]=thetaj
        #x[3]=dthetaj
        #x[4]=ddthetaj
        #
        #Output parameters
        #
        #y[1]=Re[ddB]
        #y[2]=Im[ddB]
        #
        pi=np.pi;cos=np.cos;sin=np.sin
        ddB=np.array([[x[0]*x[3]*cos(x[1]+pi/2)+x[0]*(x[2]**2)*cos(x[1]+pi)],[x[0]*x[3]*sin(x[1]+pi/2)+x[0]*(x[2]**2)*sin(x[1]+pi)]]);
        y=ddB;
        return y
    def rdy(self,x):
        #Dynamic analysis for crank
        #Copyright 2003 Qu Xiuquan
        #Input parameters
        #
        #x[0]theta -i
        #x[1]=dtheta-i
        #x[2]=ddtheta-i
        #x[3]=RxB
        #x[4]=RyB
        #
        #Output parameters
        #
        #y[0]=RxA
        #y[1]=RyA
        #y[2]=M1
        #
        pi=np.pi;cos=np.cos;sin=np.sin
        g=9.8;
        ri=self.曲柄长度;rci=self.曲柄质心;
        mi=self.曲柄质量;Ji=self.曲柄转动惯量;
        Fxi=self.曲柄外力x;Fyi =self.曲柄外力y;Mi =self.曲柄受的扭矩;
        ReddA =self.曲柄起点加速度x;ImddA =self.曲柄起点加速度y;
        y=np.array([0.001,0.0001,0.0001])
        y[0]=mi*ReddA+mi*rci*x[2]*cos(x[0]+pi/2)+mi*rci*x[1]**2*cos(x[0]+pi)-Fxi+x[3];
        y[1]=mi*ImddA+mi*rci*x[2]*sin(x[0]+pi/2)+mi*rci*x[1]**2*sin(x[0]+pi)-Fyi+x[4]+mi*g;
        y[2]=Ji*x[2]-y[0]*rci*sin(x[0])+y[1]*rci*cos(x[0])-x[3]*(ri-rci)*sin(x[0])+x[4]*(ri-rci)*cos(x[0])-Mi;
        '''y[0]=mi*ReddA+mi*rci*x[2]*cos(x[0]+pi/2)+mi*rci*x[1]**2*cos(x[0]+pi)-Fxi-x[3];
        y[1]=mi*ImddA+mi*rci*x[2]*sin(x[0]+pi/2)+mi*rci*x[1]**2*sin(x[0]+pi)-Fyi-x[4]+mi*g;
        y[2]=Ji*x[2]-y[0]*rci*sin(x[0])+y[1]*rci*cos(x[0])+x[3]*(ri-rci)*sin(x[0])-x[4]*(ri-rci)*cos(x[0])-Mi;'''
        return y
    def RRP(self,x):
        #function to compute the acceleration for RRP bar group
        #
        #Copyright 2002 Qu Xiuquan
        #
        #Input parameters
        #
        #x[0]=i
        #x[1]=theta -i
        #x[2]=theta-j
        #x[3]=dtheta-i
        #x[4]=Re[ddB]
        #x[5]=Im[ddB]
        #x[6]=Re[ddK]
        #x[7]=Im[ddk]
        #x[8]=ds
        #Output parameters
        #
        #y[0]=ddthe地-i
        #y[1]=d地
        #
        pi=np.pi;cos=np.cos;sin=np.sin
        a=np.array([[x[0]*cos(x[1]+pi/2),-cos(x[2])],[x[0]*sin(x[1]+pi/2),-sin(x[2])]]);
        b=(-np.array([[x[0]*cos(x[1]+pi),0], \
                     [x[0]*sin(x[1]+pi),0]])).dot( \
            np.array([[x[3]**2],[x[8]]])) \
                +np.array([[x[6]-x[4]],[x[7]-x[5]]])
        y=np.linalg.inv(a).dot(b);
        return y
    def RRPdy(self,x):
        #Dynamic analysis for RRP bar group
        #Copyright 2003 Qu Xiuquan
        #Input parameters
        #x(1)=theta-i
        #x(2)=dtheta-i
        #x(3)=ddtheta-i
        #x(4)=dd-j
        #x(5)=Re[ddB]
        #x(6)=Im[ddB]
        #Output parameters
        #
        #y(1)=RxB
        #y(2)=RyB
        #y(3)=RxC
        #y(4)=RyC
        #y(5)=RD
        #
        pi=np.pi;cos=np.cos;sin=np.sin
        g=9.8;
        ri=self.连杆长度;rci=self.连杆质心;mi=self.连杆质量;mj=self.滑块质量;Ji= self.连杆转动惯量;
        Fxi=self.连杆外力x;Fyi=self.连杆外力y;Fxj=self.滑块外力x;Fyj=self.滑块外力y;Mi=self.连杆受扭矩;
        thj=self.滑块和固定点延长线夹角;#
        a =np.zeros((5,5));
        a[0,0]=1;a[0,2]=1;
        a[1,1]=1;a[1,3]=1;
        a[2,0]=rci*sin(x[0]);
        a[2,1]=-rci*cos(x[0]);
        a[2,2]=-(ri-rci)*sin(x[0]);
        a[2,3]=(ri-rci)*cos(x[0]);
        a[3,2]=-1;a[3,4]=-sin(thj);
        a[4,3]=-1;a[4,4]=cos(thj)
        b=np.zeros((5,1));
        b[0,0]=mi*x[4]+mi*rci*x[2]*cos(x[0]+pi/2)+mi*rci*x[1]**2*cos(x[0]+pi)-Fxi;
        b[1,0]=mi*x[5]+mi*rci*x[2]*sin(x[0]+pi/2)+mi*rci*x[1]**2*sin(x[0]+pi)-Fyi+mi*g;
        b[2,0]=Ji*x[2]-Mi;
        b[3,0]=mj*x[3]*cos(thj)-Fxj;
        b[4,0]=mj*x[3]*sin(thj)-Fyj+mj*g;
        y=np.linalg.inv(a).dot(b);
        return y
    def 曲柄角速度变化(self,曲柄角速度):
        if self.曲柄角速度<0.2:
           self.曲柄角速度=曲柄角速度+0.0001
        else:self.曲柄角速度=曲柄角速度
        return
    def f(self,x,a,b):#速度时间关系函数
        return a*x + b
    def 积分(self,积分距离,a,b):
        v = integrate.quad(self.f,0,积分距离,args=(a,b))
        return v[0]
    def 合成仿真(self):
        时间=np.linspace(0,self.模拟时间长度,self.时间精度)
        时间间隔=self.模拟时间长度/self.时间精度
        print(len(时间))
        数据=pd.DataFrame()
        数据['时间']=时间
        #rx=[rj,]
        #曲柄角速度=self.曲柄角速度
        曲柄转角=self.滑块和固定点延长线夹角;theta2=self.滑块和固定点延长线夹角;dtheta2=-self.曲柄角速度/(self.连杆长度/self.曲柄长度);滑块速度=0;
        滑块位移=0
        for i in range(len(时间)):
            self.曲柄角速度变化(self.曲柄角速度)
            ddb=self.r(np.array([self.曲柄长度,曲柄转角,self.曲柄角速度,0]))
            数据.loc[i,'bx方向加速度']=float(ddb[0])
            数据.loc[i,'by方向加速度']=float(ddb[1])                       
            print(float(ddb[0]),float(ddb[1]))
            s=self.RRP(np.array([self.连杆长度,theta2,self.滑块和固定点延长线夹角,dtheta2,float(ddb[0]),float(ddb[1]),0,0,滑块速度]))
            数据.loc[i,'ddtheta2']=float(s[0])
            数据.loc[i,'dds']=float(s[1])
            print(i)
            print(s[0])
            rrpdy=self.RRPdy(np.array([theta2,dtheta2,float(s[0]),float(s[1]),float(ddb[0]),float(ddb[1])]))
            rdy=self.rdy(np.array([曲柄转角,self.曲柄角速度,0,-float(rrpdy[0]),-float(rrpdy[1])]))
            曲柄转角=self.积分(时间间隔,0,self.曲柄角速度)+曲柄转角
            dtheta2=self.积分(时间间隔,0,float(s[0]))+dtheta2     
            theta2=self.积分(时间间隔,0,dtheta2)+theta2 #这句可能也有问题
            滑块速度=self.积分(时间间隔,0,float(s[1]))+滑块速度
            #滑块位移=self.积分(时间间隔,0,float(滑块速度))+滑块位移#这句有问题
            滑块位移=5
            数据.loc[i,'曲柄转角']=float(曲柄转角)*180/np.pi-270
            数据.loc[i,'滑块速度']=float(滑块速度)
            数据.loc[i,'滑块位移']=float(滑块位移)  
            数据.loc[i,'连杆角速度']=float(dtheta2)
            数据.loc[i,'连杆角']=float(theta2)
            数据.loc[i,'曲柄约束力bx'] = float(rdy[0])
            数据.loc[i,'曲柄约束力by'] = float(rdy[1])
            曲柄力=np.sqrt(rdy[0]**2+rdy[1]**2)
            数据.loc[i,'曲柄力'] = float(曲柄力)
            数据.loc[i,'曲柄扭矩']=float(rdy[2])
            数据.loc[i,'连杆支撑力bx']=float(rrpdy[0])
            数据.loc[i,'连杆支撑力by'] = float(rrpdy[1])
            数据.loc[i,'连杆支撑力cx'] = float(rrpdy[2])
            数据.loc[i,'连杆支撑力cy'] = float(rrpdy[3])
            数据.loc[i,'滑块侧向力'] = float(rrpdy[4])
        #数据.loc[i,'曲柄转角']=数据.loc[i,'曲柄转角']*180/np.pi-270
        return 数据
if __name__=="__main__":
    '''a=曲柄连杆(连杆长度=1.231,曲柄长度=0.175,连杆质心=0.6,曲柄质心=0.09,曲柄质量=10699,连杆质量=7350,滑块质量=25200, \
                 曲柄转动惯量=592,连杆转动惯量=1787,滑块外力x=0,滑块外力y=25000000)'''
    a=曲柄连杆(连杆长度=1.231,曲柄长度=0.175,连杆质心=0.6,曲柄质心=0.09,曲柄质量=0.034,连杆质量=1.94,滑块质量=62, \
                 曲柄转动惯量=9.3e-5,连杆转动惯量=0.25,滑块外力x=0,滑块外力y=50000)
    '''a=曲柄连杆(连杆长度=1.2,曲柄长度=0.4,连杆质心=0.6,曲柄质心=0.2,曲柄质量=1.2,连杆质量=3.6,滑块质量=6, \
                 曲柄转动惯量=0.016,连杆转动惯量=0.45,滑块外力x=1000,滑块外力y=0)'''
    y=a.合成仿真()
    #sv.buildGUI(y)
    #plt.plot(y.曲柄转角,y.rdy)
    #plt.show()
    y.滑块位移=y.滑块位移-min(y.滑块位移)
    DATA =y ## Put Your DataFrame Object name here ##  

    #- Axes Setting ---------- 
    fig, ax = plt.subplots()
    ax.set_xlabel( "曲柄转角")
    ax.set_ylabel( "滑块位移" )
    ax.set_xlim(min(DATA['曲柄转角'].replace([np.inf, -np.inf], np.nan ).dropna() ) - abs( min(DATA['曲柄转角'].replace([np.inf, -np.inf], np.nan ).dropna() )/10), max(DATA['曲柄转角'].replace([np.inf, -np.inf], np.nan).dropna()) + abs(max(DATA['曲柄转角'].replace([np.inf, -np.inf], np.nan).dropna())/10)  )
    ax.set_ylim( min(DATA['滑块位移'].replace([np.inf, -np.inf], np.nan ).dropna() ) - abs( min(DATA['滑块位移'].replace([np.inf, -np.inf], np.nan ).dropna() )/10), max(DATA['滑块位移'].replace([np.inf, -np.inf], np.nan).dropna()) + abs(max(DATA['滑块位移'].replace([np.inf, -np.inf], np.nan).dropna())/10)  )
    #- PLOT ------------------ 
    ax.scatter(DATA["曲柄转角"].replace([np.inf, -np.inf], np.nan), DATA["滑块位移"].replace([np.inf, -np.inf], np.nan), s = 5.0, alpha =1.0,edgecolor="black",linewidth= 0.0)
    plt.show() 
    fig, ax = plt.subplots()
    ax.set_xlabel( "曲柄转角")
    ax.set_ylabel( "曲柄扭矩" )
    ax.set_xlim(min(DATA['曲柄转角'].replace([np.inf, -np.inf], np.nan ).dropna() ) - abs( min(DATA['曲柄转角'].replace([np.inf, -np.inf], np.nan ).dropna() )/10), max(DATA['曲柄转角'].replace([np.inf, -np.inf], np.nan).dropna()) + abs(max(DATA['曲柄转角'].replace([np.inf, -np.inf], np.nan).dropna())/10)  )
    ax.set_ylim( min(DATA['曲柄扭矩'].replace([np.inf, -np.inf], np.nan ).dropna() ) - abs( min(DATA['曲柄扭矩'].replace([np.inf, -np.inf], np.nan ).dropna() )/10), max(DATA['曲柄扭矩'].replace([np.inf, -np.inf], np.nan).dropna()) + abs(max(DATA['曲柄扭矩'].replace([np.inf, -np.inf], np.nan).dropna())/10)  )
    #- PLOT ------------------ 
    ax.scatter(DATA["曲柄转角"].replace([np.inf, -np.inf], np.nan), DATA["曲柄扭矩"].replace([np.inf, -np.inf], np.nan), s = 5.0, alpha =1.0,edgecolor="black",linewidth= 0.0)
    plt.show() 
    fig, ax = plt.subplots()
    ax.set_xlabel( "时间")
    ax.set_ylabel( "滑块速度" )
    ax.set_xlim(min(DATA['时间'].replace([np.inf, -np.inf], np.nan ).dropna() ) - abs( min(DATA['时间'].replace([np.inf, -np.inf], np.nan ).dropna() )/10), max(DATA['时间'].replace([np.inf, -np.inf], np.nan).dropna()) + abs(max(DATA['时间'].replace([np.inf, -np.inf], np.nan).dropna())/10)  )
    ax.set_ylim( min(DATA['滑块速度'].replace([np.inf, -np.inf], np.nan ).dropna() ) - abs( min(DATA['滑块速度'].replace([np.inf, -np.inf], np.nan ).dropna() )/10), max(DATA['滑块速度'].replace([np.inf, -np.inf], np.nan).dropna()) + abs(max(DATA['滑块速度'].replace([np.inf, -np.inf], np.nan).dropna())/10)  )
    #- PLOT ------------------ 
    ax.scatter(DATA["时间"].replace([np.inf, -np.inf], np.nan), DATA["滑块速度"].replace([np.inf, -np.inf], np.nan), s = 5.0, alpha =1.0,edgecolor="black",linewidth= 0.0)
    plt.show() 
    fig, ax = plt.subplots()
    ax.set_xlabel( "曲柄转角")
    ax.set_ylabel( "连杆支撑力bx" )
    ax.set_xlim(min(DATA['曲柄转角'].replace([np.inf, -np.inf], np.nan ).dropna() ) - abs( min(DATA['曲柄转角'].replace([np.inf, -np.inf], np.nan ).dropna() )/10), max(DATA['曲柄转角'].replace([np.inf, -np.inf], np.nan).dropna()) + abs(max(DATA['曲柄转角'].replace([np.inf, -np.inf], np.nan).dropna())/10)  )
    ax.set_ylim( min(DATA['连杆支撑力bx'].replace([np.inf, -np.inf], np.nan ).dropna() ) - abs( min(DATA['连杆支撑力bx'].replace([np.inf, -np.inf], np.nan ).dropna() )/10), max(DATA['连杆支撑力bx'].replace([np.inf, -np.inf], np.nan).dropna()) + abs(max(DATA['连杆支撑力bx'].replace([np.inf, -np.inf], np.nan).dropna())/10)  )
    #- PLOT ------------------ 
    ax.scatter(DATA["曲柄转角"].replace([np.inf, -np.inf], np.nan), DATA["连杆支撑力bx"].replace([np.inf, -np.inf], np.nan), s = 5.0, alpha =1.0,edgecolor="black",linewidth= 0.0)
    plt.show() 